// #####################################
// File     : motor.c
// Author   : Chris Meaclem, Isaac, Andrew
// Date     : 28/07/2011
// Release  : Intermediate
// Purpose  : Library to provide control for motors via motor driver
// Notes    : Motor Driver connected to PORTB.
// #####################################


/*
http://www.electronicsblog.net/atmega8-pwm-controlfrequencypolarityduty-cycle-example-tutorial-with-program/
https://sites.google.com/site/dlangenberg2/atmega8basics
*/

/*Includes:*/
#include "motor.h"
#include <avr/io.h>
//#include "ErrorCodes.h"

/*END - Includes:*/
/*Definitions:*/
#define RESET_PORT() PORTB=0x00
	//RESET_PORT() : Reset PORTB to zeros to allow action to be set in TWO seperate calls.

#define BITMASK_LEFT(Enable, Forwards, Reverse) ((Enable)<<DDB5)|((Forwards)<<DDB4)|((Reverse)<<DDB3)
#define BITMASK_RIGHT(Enable, Forwards, Reverse) ((Enable)<<DDB0)|((Forwards)<<DDB1)|((Reverse)<<DDB2)
	//BITMASK: Provides the port operations for the LEFT and RIGHT motors.
	
	
//#define HARDTURNS
	//HARDTURNS - Define to use hard turning, otherwise regular turning will be used.
#define QUICKSTOP
	//QUICKSTOP - Define to use a faster stopping procedure. sets PORTB low with only one operation


//typedef struct {uint pinEn, uint pinFor, uint pinRev} motor_t;//Define a satatype for the motor object
enum motorStates {motor_Stop = 0, motor_For, motor_Rev};/*States the motor can be in*/

/*END - Definitions:*/

static uint motor_Set(enum motorID motor, enum motorStates state);


/*Globals:*/
//motor_t motorLeft, motorRight;

/*END - Globals:*/


void motor_Init(void)
{
	/* Description:
	* Initilise the motors
	* Scope: Global.
	* Input: left and right indicate what wheel. En=Enable pin, For=Forwards pin, Rev=Reverse Pin.
	*/
	
	//Setup the data direction register
	DDRB = 0xFF; //set PORTB as output
	
	//initilise pins to low (off)
	PORTB = 0x00;
}

uint vechile_Drive(enum vechileStates state)
{
	/* Description:
	* function to execute desired motor actions. Uses 'motor_Set to actuate the desired state
	* Scope: Global.
	* Input: 'state' belongs to 'vechileStates'
	*/
	
	switch(state)
	{
		//Is it worth adding a 'hard' turn feature to turn faster? Would have smaller turning radius...
		case(vechile_Stop):
			motor_Set(motor_BothID, motor_Stop);
			break;
			
		case(vechile_For):
			motor_Set(motor_BothID, motor_For);
			break;
			
		case(vechile_Rev):
			motor_Set(motor_BothID, motor_Rev);
			break;
			
		case(vechile_Right):
		/*Implement regular turn:*/
				#ifndef HARDTURNS
					RESET_PORT();//Reset PORTB to zeros to allow action to be set in TWO seperate calls.
					motor_Set(motor_LeftID, motor_For);
					motor_Set(motor_RightID, motor_Stop);
				#endif
				
			/*Implement a hard left turn:*/
				#ifdef HARDTURNS
					RESET_PORT();//Reset PORTB to zeros to allow action to be set in TWO seperate calls.
					motor_Set(motor_LeftID, motor_For);
					motor_Set(motor_RightID, motor_Rev);
				#endif
			break;
			
		case(vechile_Left):
			/*Implement regular turn:*/
				#ifndef HARDTURNS
					RESET_PORT();//Reset PORTB to zeros to allow action to be set in TWO seperate calls.
					motor_Set(motor_LeftID, motor_Stop);
					motor_Set(motor_RightID, motor_For);
				#endif
			
			/*Implement a hard left turn:*/
				#ifdef HARDTURNS
				RESET_PORT();//Reset PORTB to zeros to allow action to be set in TWO seperate calls.
				motor_Set(motor_LeftID, motor_Rev);
				motor_Set(motor_RightID, motor_For);
				#endif
			break;
			

		default:
			//Error case, do something here to alert
			//return E_INVALID_ARG;
			break;
	}
	//return E_NONE;
	return 0;
}

static uint motor_Set(enum motorID motor, enum motorStates state)
{
	/* Description: Fxn to execute desired motor actions on specifed wheel
	* Scope: Internal.
	* Input: 'motor' indicates the left or right motor
	* Input: 'state' indicates the state to set the motor
	*/

	
	switch(state)
	{
		case(motor_Stop):
				if(motor == motor_LeftID)
				{
					
					//PORTB = ENABLE|FOR|REV
					//PORTB = PORTB | (0<<DDB5)|(0<<DDB4)|(0<<DDB3);
					PORTB = PORTB | BITMASK_LEFT(0,0,0);
				}
				else if(motor == motor_RightID)
				{		
					
					//PORTB = ENABLE|FOR|REV
					//PORTB = PORTB | (0<<DDB0)|(0<<DDB1)|(0<<DDB2);
					PORTB = PORTB | BITMASK_RIGHT(0,0,0);
				}
				else if(motor == motor_BothID)
				{				
					//PORTB = ENABLE|FOR|REV
					
					//PORTB = (0<<DDB5)|(0<<DDB4)|(0<<DDB3)  |  (0<<DDB0)|(0<<DDB1)|(0<<DDB2);//left|right
					
					/*Implement regular stop:*/
						#ifndef QUICKSTOP
							//PORTB = (0<<DDB5)|(0<<DDB4)|(0<<DDB3)  |  (0<<DDB0)|(0<<DDB1)|(0<<DDB2);//left|right
							PORTB = BITMASK_LEFT(0,0,0) | BITMASK_RIGHT(0,0,0);
						#endif
					
					/*Implement a quick stop:*/
						//Saves having to do bitshifts.
						#ifdef QUICKSTOP
							RESET_PORT();//Set PORTB to low straight away.
						#endif
				}
			break;
			
		case(motor_For):
				if(motor == motor_LeftID)
				{
					//PORTB = ENABLE|FOR|REV
					//PORTB = (1<<DDB5)|(1<<DDB4)|(0<<DDB3);
					//PORTB = PORTB | (1<<DDB5)|(1<<DDB4)|(0<<DDB3);
					PORTB = PORTB | BITMASK_LEFT(1,1,0);
				}
				else if(motor == motor_RightID)
				{				
					//PORTB = ENABLE|FOR|REV
					//PORTB = (1<<DDB0)|(1<<DDB1)|(0<<DDB2);
					//PORTB = PORTB | (1<<DDB0)|(1<<DDB1)|(0<<DDB2);
					PORTB = PORTB | BITMASK_RIGHT(1,1,0);
				}
				else if(motor == motor_BothID)
				{				
					//PORTB = ENABLE|FOR|REV
					//PORTB = (1<<DDB5)|(1<<DDB4)|(0<<DDB3)  |  (1<<DDB0)|(1<<DDB1)|(0<<DDB2);//left|right
					PORTB = BITMASK_LEFT(1,1,0)  |  BITMASK_RIGHT(1,1,0);//left|right
				}
			break;
				
		case(motor_Rev):
				if(motor == motor_LeftID)
				{
					//PORTB = ENABLE|FOR|REV
					//PORTB = PORTB | (1<<DDB5)|(0<<DDB4)|(1<<DDB3);
					PORTB = PORTB | BITMASK_LEFT(1,0,1);
				}
				else if(motor == motor_RightID)
				{				
					//PORTB = ENABLE|FOR|REV
					//PORTB = PORTB | (1<<DDB0)|(0<<DDB1)|(1<<DDB2);
					PORTB = PORTB | BITMASK_RIGHT(1,0,1);
				}
				else if(motor == motor_BothID)
				{				
					//PORTB = ENABLE|FOR|REV
					//PORTB = (1<<DDB5)|(0<<DDB4)|(1<<DDB3)  |  (1<<DDB0)|(0<<DDB1)|(1<<DDB2);//left|right
					PORTB = BITMASK_LEFT(1,0,1)  |  BITMASK_RIGHT(1,0,1);//left|right
				}
			break;
		default:
			//Error case, do something here to alert
			//return E_INVALID_ARG;
			break;
	}
	//return E_NONE;
	return 0;
	
}


